#include <vector>
#include <map>
#include <string>
#include <functional>

using namespace std;

//******************************
//          结构体定义
//******************************
    /*struct: Pose
     * 表示位置的结构体
     * 1 左上角为原点，横向为x，纵向为y
     */
    struct Pose
    {
        Pose(){}
        Pose(double _x, double _y, int _id){x=_x; y=_y; id=_id;}

        double x = 0;     //位置x
        double y = 0;     //位置y
        int   id = 0;       
    };

    /*struct: Vertex/Edge
     * 表示道路相关信息的结构体
     * 1 用于表示道路的顶点、边
     */
    struct Vertex
    {
        Vertex(){}
        Vertex(vector<int> related_vertexes, vector<int> related_edges, Pose vertex_pos)
        {
            relatedVertexes = related_vertexes;
            relatedEdges    = related_edges;
            vertexPos         = vertex_pos;
        }

        vector<int> relatedVertexes;        //与该顶点关联的所有顶点和对应的边的ID，是否关联，根据车道标志（左转等）确定
        vector<int> relatedEdges;
        Pose vertexPos;                 //该顶点的位置和ID
    };

    struct Edge
    {
        Edge(){}
        Edge(int id, vector<int> related_vertexes, int type,
                    double dis, double angle, int cost, bool isReverse = false)
        {
            relatedVertexes.clear();
            relatedVertexes = related_vertexes;
            
            attr_id         = id;
            attr_type       = type;
            attr_dis        = dis;
            attr_angle      = angle;
            attr_cost       = cost;
            attr_isReverse  = isReverse;
        }

        vector<int> relatedVertexes;        //该道路两端的顶点ID
        int attr_id;                        //边属性0：id
        int attr_type;                      //边属性1：1代表直行，2代表左转弯，3代表右转弯
        double attr_dis;                    //边属性2：直行时代表路段长度，转弯时代表路段半径，单位：mm
        double attr_angle;                  //边属性3：直行时取0，转弯时取转弯的角度，单位：度
        int attr_cost;                      //边属性4：计算最短路径时使用的代价值
        bool attr_isReverse;                //边属性5：入库时，是否属于倒车边，车库外的边默认false
    };

    
//******************************
//          全局变量
//******************************


//定义车辆信息
    vector<string> vehicleInfoS{"car","car","car","car","car","bus","bus","bus","van","truck"};

    const double dx = -918, dy = -106, di = 250;
    const double theta_bias = -0.0032; // 坐标轴偏差的角度，为正是使规划出的轨迹的坐标顺时针旋转

    // const double dx = -918, dy = -46;
    //定义信号灯信息   
    //                              {xmin,xmax,ymin,ymax}
    vector<vector<double>> TLInfoS{ {0+dx,224+dx,214+dy,862+di+dy},
                                    {441+dx,852+di+dx,214+dy,440+dy},
                                    {213+dx,756+di+dx,3566+dy,3792+dy},
                                    {214+dx,439+dx,2941-di+dy,3351+dy},
                                    {2786+dx,3162+di+dx,1778+dy,2004+dy},
                                    {2556+dx,2787+dx,2877-di+dy,3271+dy},
                                    {5154+dx,5378+dx,1777+dy,2397+di+dy},
                                    {4732-di+dx,5377+dx,1474+dy,1774+dy},
                                    {5377+dx,5593+dx,1103-di+dy,1775+dy} }; 

// 定义实际地图坐标原点与场景地图左下角之间的坐标差,unit:mm


//定义拓扑地图的顶点  
    //                              { {可到达的顶点}, {对应的边},  Pose{x,y,id}}
    vector<Vertex> mapVertexInfoS{  
                                    // { {2}, {3}, Pose(3654+dx, 800+dy, 1) }
                                    // ,{ {1, 3}, {2, 4}, Pose(3654+dx, 874+dy, 2) }
                                    // ,{ {2, 4}, {1, 5}, Pose(3836+dx, 1074+dy, 3) }
                                    // ,{ {5}, {6}, Pose(4186+dx, 1074+dy, 4) }
                                    // ,{ {6}, {7}, Pose(4386+dx, 1274+dy, 5) }
                                    // ,{ {7}, {8}, Pose(4697+dx, 1581+dy, 6) }
                                    // ,{ {8}, {9}, Pose(5485+dx, 2404+dy, 7) }
                                    // ,{ {9}, {10}, Pose(5485+dx, 2651+dy, 8) }
                                    // ,{ {10}, {11}, Pose(4752+dx, 3384+dy, 9) }
                                    // ,{ {11}, {12}, Pose(3983+dx, 3384+dy, 10) }
                                    // ,{ {12}, {13}, Pose(3654+dx, 3588+dy, 11) }
                                    // ,{ {13, 41}, {14, 44}, Pose(3186+dx, 3685+dy, 12) }
                                    // ,{ {14}, {15}, Pose(1942+dx, 3685+dy, 13) }
                                    // ,{ {15}, {16}, Pose(840+dx, 3685+dy, 14) }
                                    // ,{ {16}, {17}, Pose(107+dx, 2943+dy, 15) }
                                    // ,{ {17}, {18}, Pose(107+dx, 840+dy, 16) }
                                    // ,{ {18}, {19}, Pose(840+dx, 107+dy, 17) }
                                    // ,{ {19}, {20}, Pose(1942+dx, 107+dy, 18) }
                                    // ,{ {20}, {21}, Pose(2679+dx, 840+dy, 19) }
                                    // ,{ {21}, {22}, Pose(2679+dx, 1074+dy, 20) }
                                    // ,{ {22}, {23}, Pose(3186+dx, 1581+dy, 21) }
                                    // ,{ {23, 6}, {24, 46}, Pose(3329+dx, 1581+dy, 22) }
                                    // ,{ {3}, {25}, Pose(3636+dx, 1274+dy, 23) }
                                    // ,{ {25, 34}, {26, 47}, Pose(3186+dx, 1897+dy, 24) }
                                    // ,{ {26}, {27}, Pose(2679+dx, 2404+dy, 25) }
                                    // ,{ {27, 13}, {28, 48}, Pose(2679+dx, 2877+dy, 26) }
                                    // ,{ {28}, {29}, Pose(3186+dx, 3384+dy, 27) }
                                    // ,{ {29}, {30}, Pose(3591+dx, 3308+dy, 28) }
                                    // ,{ {30}, {31}, Pose(3901+dx, 3158+dy, 29) }
                                    // ,{ {31}, {32}, Pose(4752+dx, 3158+dy, 30) }
                                    // ,{ {32}, {33}, Pose(5259+dx, 2651+dy, 31) }
                                    // ,{ {33}, {34}, Pose(5259+dx, 2404+dy, 32) }
                                    // ,{ {24}, {35}, Pose(4697+dx, 1897+dy, 33) }
                                    // ,{ {35}, {36}, Pose(2453+dx, 840+dy, 34) }
                                    // ,{ {36}, {37}, Pose(1942+dx, 333+dy, 35) }
                                    // ,{ {37}, {38}, Pose(840+dx, 333+dy, 36) }
                                    // ,{ {38}, {39}, Pose(333+dx, 840+dy, 37) }
                                    // ,{ {39}, {40}, Pose(333+dx, 2943+dy, 38) }
                                    // ,{ {40}, {41}, Pose(840+dx, 3459+dy, 39) }
                                    // ,{ {41, 27}, {42, 45}, Pose(1942+dx, 3459+dy, 40) }
                                    // ,{ {34}, {43}, Pose(2453+dx, 2877+dy, 41) }
                                    // 加密后改变的初始的点
                                        { {2}, {3}, Pose(3654+dx, 800+dy, 1) }
                                        ,{ {1, 3}, {2, 4}, Pose(3654+dx, 874+dy, 2) }
                                        ,{ {2, 42}, {1, 5}, Pose(3836+dx, 1074+dy, 3) }
                                        // changed!
                                        ,{ {5}, {6}, Pose(4186+dx, 1074+dy, 4) }
                                        ,{ {6}, {7}, Pose(4386+dx, 1274+dy, 5) }
                                        ,{ {7}, {8}, Pose(4697+dx, 1581+dy, 6) }
                                        ,{ {43}, {9}, Pose(5485+dx, 2404+dy, 7) }
                                        // changed!
                                        ,{ {9}, {10}, Pose(5485+dx, 2651+dy, 8) }
                                        ,{ {44}, {11}, Pose(4752+dx, 3384+dy, 9) }
                                        // changed!
                                        ,{ {47}, {12}, Pose(3983+dx, 3384+dy, 10) }
                                        // changed!
                                        ,{ {48}, {13}, Pose(3654+dx, 3588+dy, 11) }
                                        // changed!
                                        ,{ {50, 41}, {14, 44}, Pose(3186+dx, 3685+dy, 12) }
                                        // changed!
                                        ,{ {56}, {15}, Pose(1942+dx, 3685+dy, 13) }
                                        // changed!
                                        ,{ {15}, {16}, Pose(840+dx, 3685+dy, 14) }
                                        ,{ {61}, {17}, Pose(107+dx, 2943+dy, 15) }
                                        // changed!
                                        ,{ {17}, {18}, Pose(107+dx, 840+dy, 16) }
                                        ,{ {71}, {19}, Pose(840+dx, 107+dy, 17) }
                                        // changed!
                                        ,{ {19}, {20}, Pose(1942+dx, 107+dy, 18) }
                                        ,{ {76}, {21}, Pose(2679+dx, 840+dy, 19) }
                                        // changed!
                                        ,{ {21}, {22}, Pose(2679+dx, 1074+dy, 20) }
                                        ,{ {22}, {23}, Pose(3186+dx, 1581+dy, 21) }
                                        ,{ {23, 130}, {24, 46}, Pose(3329+dx, 1581+dy, 22) }
                                        // changed!
                                        ,{ {3}, {25}, Pose(3636+dx, 1274+dy, 23) }
                                        ,{ {25, 34}, {26, 47}, Pose(3186+dx, 1897+dy, 24) }
                                        ,{ {77}, {27}, Pose(2679+dx, 2404+dy, 25) }
                                        // changed!
                                        ,{ {27, 13}, {28, 48}, Pose(2679+dx, 2877+dy, 26) }
                                        ,{ {79}, {29}, Pose(3186+dx, 3384+dy, 27) }
                                        // changed!
                                        ,{ {81}, {30}, Pose(3591+dx, 3308+dy, 28) }
                                        // changed!
                                        ,{ {82}, {31}, Pose(3901+dx, 3158+dy, 29) }
                                        // changed!
                                        ,{ {31}, {32}, Pose(4752+dx, 3158+dy, 30) }
                                        ,{ {86}, {33}, Pose(5259+dx, 2651+dy, 31) }
                                        // changed!
                                        ,{ {33}, {34}, Pose(5259+dx, 2404+dy, 32) }
                                        ,{ {87}, {35}, Pose(4697+dx, 1897+dy, 33) }
                                        // changed!
                                        ,{ {35}, {36}, Pose(2453+dx, 840+dy, 34) }
                                        ,{ {94}, {37}, Pose(1942+dx, 333+dy, 35) }
                                        // changed!
                                        ,{ {37}, {38}, Pose(840+dx, 333+dy, 36) }
                                        ,{ {99}, {39}, Pose(333+dx, 840+dy, 37) }
                                        // changed!
                                        ,{ {39}, {40}, Pose(333+dx, 2943+dy, 38) }
                                        ,{ {109}, {41}, Pose(840+dx, 3459+dy, 39) }
                                        // changed!
                                        ,{ {41, 124}, {42, 45}, Pose(1942+dx, 3459+dy, 40) }
                                        // changed!
                                        ,{ {114}, {43}, Pose(2453+dx, 2877+dy, 41) }
                                        // changed! 
                                    // 加密获得的点
                                        ,{ {4}, {49}, Pose(4036+dx, 1074+dy, 42) }
                                        ,{ {8}, {50}, Pose(5485+dx, 2604+dy, 43) }
                                        ,{ {45}, {51}, Pose(4552+dx, 3384+dy, 44) }
                                        ,{ {46}, {52}, Pose(4352+dx, 3384+dy, 45) }
                                        ,{ {10}, {53}, Pose(4152+dx, 3384+dy, 46) }
                                        ,{ {11}, {54}, Pose(3813+dx, 3489+dy, 47) }
                                        ,{ {49}, {55}, Pose(3458+dx, 3628+dy, 48) }
                                        ,{ {12}, {56}, Pose(3262+dx, 3668+dy, 49) }
                                        ,{ {51}, {57}, Pose(2986+dx, 3685+dy, 50) }
                                        ,{ {52}, {58}, Pose(2786+dx, 3685+dy, 51) }
                                        ,{ {53}, {59}, Pose(2586+dx, 3685+dy, 52) }
                                        ,{ {54}, {60}, Pose(2386+dx, 3685+dy, 53) }
                                        ,{ {55}, {61}, Pose(2186+dx, 3685+dy, 54) }
                                        ,{ {13}, {62}, Pose(1986+dx, 3685+dy, 55) }
                                        ,{ {57}, {63}, Pose(1742+dx, 3685+dy, 56) }
                                        ,{ {58}, {64}, Pose(1542+dx, 3685+dy, 57) }
                                        ,{ {59}, {65}, Pose(1342+dx, 3685+dy, 58) }
                                        ,{ {60}, {66}, Pose(1142+dx, 3685+dy, 59) }
                                        ,{ {14}, {67}, Pose(942+dx, 3685+dy, 60) }
                                        ,{ {62}, {68}, Pose(107+dx, 2743+dy, 61) }
                                        ,{ {63}, {69}, Pose(107+dx, 2543+dy, 62) }
                                        ,{ {64}, {70}, Pose(107+dx, 2343+dy, 63) }
                                        ,{ {65}, {71}, Pose(107+dx, 2143+dy, 64) }
                                        ,{ {66}, {72}, Pose(107+dx, 1943+dy, 65) }
                                        ,{ {67}, {73}, Pose(107+dx, 1743+dy, 66) }
                                        ,{ {68}, {74}, Pose(107+dx, 1543+dy, 67) }
                                        ,{ {69}, {75}, Pose(107+dx, 1343+dy, 68) }
                                        ,{ {70}, {76}, Pose(107+dx, 1143+dy, 69) }
                                        ,{ {16}, {77}, Pose(107+dx, 943+dy, 70) }
                                        ,{ {72}, {78}, Pose(1040+dx, 107+dy, 71) }
                                        ,{ {73}, {79}, Pose(1240+dx, 107+dy, 72) }
                                        ,{ {74}, {80}, Pose(1440+dx, 107+dy, 73) }
                                        ,{ {75}, {81}, Pose(1640+dx, 107+dy, 74) }
                                        ,{ {18}, {82}, Pose(1840+dx, 107+dy, 75) }
                                        ,{ {20}, {83}, Pose(2679+dx, 1040+dy, 76) }
                                        ,{ {78}, {84}, Pose(2679+dx, 2604+dy, 77) }
                                        ,{ {26}, {85}, Pose(2679+dx, 2804+dy, 78) }
                                        ,{ {80}, {86}, Pose(3382+dx, 3347+dy, 79) }
                                        ,{ {28}, {87}, Pose(3578+dx, 3310+dy, 80) }
                                        ,{ {29}, {88}, Pose(3771+dx, 3220+dy, 81) }
                                        ,{ {83}, {89}, Pose(4101+dx, 3158+dy, 82) }
                                        ,{ {84}, {90}, Pose(4301+dx, 3158+dy, 83) }
                                        ,{ {85}, {91}, Pose(4501+dx, 3158+dy, 84) }
                                        ,{ {30}, {92}, Pose(4701+dx, 3158+dy, 85) }
                                        ,{ {32}, {93}, Pose(5259+dx, 2451+dy, 86) }
                                        ,{ {88}, {94}, Pose(4497+dx, 1897+dy, 87) }
                                        ,{ {89}, {95}, Pose(4297+dx, 1897+dy, 88) }
                                        ,{ {90}, {96}, Pose(4097+dx, 1897+dy, 89) }
                                        ,{ {91}, {97}, Pose(3897+dx, 1897+dy, 90) }
                                        ,{ {92}, {98}, Pose(3697+dx, 1897+dy, 91) }
                                        ,{ {93}, {99}, Pose(3497+dx, 1897+dy, 92) }
                                        ,{ {24}, {100}, Pose(3297+dx, 1897+dy, 93) }
                                        ,{ {95}, {101}, Pose(1742+dx, 333+dy, 94) }
                                        ,{ {96}, {102}, Pose(1542+dx, 333+dy, 95) }
                                        ,{ {97}, {103}, Pose(1342+dx, 333+dy, 96) }
                                        ,{ {98}, {104}, Pose(1142+dx, 333+dy, 97) }
                                        ,{ {36}, {105}, Pose(942+dx, 333+dy, 98) }
                                        ,{ {100}, {106}, Pose(333+dx, 1040+dy, 99) }
                                        ,{ {101}, {107}, Pose(333+dx, 1240+dy, 100) }
                                        ,{ {102}, {108}, Pose(333+dx, 1440+dy, 101) }
                                        ,{ {103}, {109}, Pose(333+dx, 1640+dy, 102) }
                                        ,{ {104}, {110}, Pose(333+dx, 1840+dy, 103) }
                                        ,{ {105}, {111}, Pose(333+dx, 2040+dy, 104) }
                                        ,{ {106}, {112}, Pose(333+dx, 2240+dy, 105) }
                                        ,{ {107}, {113}, Pose(333+dx, 2440+dy, 106) }
                                        ,{ {108}, {114}, Pose(333+dx, 2640+dy, 107) }
                                        ,{ {38}, {115}, Pose(333+dx, 2840+dy, 108) }
                                        ,{ {110}, {116}, Pose(1040+dx, 3459+dy, 109) }
                                        ,{ {111}, {117}, Pose(1240+dx, 3459+dy, 110) }
                                        ,{ {112}, {118}, Pose(1440+dx, 3459+dy, 111) }
                                        ,{ {113}, {119}, Pose(1640+dx, 3459+dy, 112) }
                                        ,{ {40}, {120}, Pose(1840+dx, 3459+dy, 113) }
                                        ,{ {115}, {121}, Pose(2453+dx, 2677+dy, 114) }
                                        ,{ {116}, {122}, Pose(2453+dx, 2477+dy, 115) }
                                        ,{ {117}, {123}, Pose(2453+dx, 2277+dy, 116) }
                                        ,{ {118}, {124}, Pose(2453+dx, 2077+dy, 117) }
                                        ,{ {119}, {125}, Pose(2453+dx, 1877+dy, 118) }
                                        ,{ {120}, {126}, Pose(2453+dx, 1677+dy, 119) }
                                        ,{ {121}, {127}, Pose(2453+dx, 1477+dy, 120) }
                                        ,{ {122}, {128}, Pose(2453+dx, 1277+dy, 121) }
                                        ,{ {123}, {129}, Pose(2453+dx, 1077+dy, 122) }
                                        ,{ {34}, {130}, Pose(2453+dx, 877+dy, 123) }
                                        ,{ {125}, {131}, Pose(2141+dx, 3446+dy, 124) }
                                        ,{ {126}, {132}, Pose(2340+dx, 3433+dy, 125) }
                                        ,{ {127}, {133}, Pose(2539+dx, 3420+dy, 126) }
                                        ,{ {128}, {134}, Pose(2738+dx, 3407+dy, 127) }
                                        ,{ {129}, {135}, Pose(2937+dx, 3394+dy, 128) }
                                        ,{ {27}, {136}, Pose(3136+dx, 3381+dy, 129) }
                                        ,{ {131}, {137}, Pose(3529+dx, 1581+dy, 130) }
                                        ,{ {132}, {138}, Pose(3729+dx, 1581+dy, 131) }
                                        ,{ {133}, {139}, Pose(3929+dx, 1581+dy, 132) }
                                        ,{ {134}, {140}, Pose(4129+dx, 1581+dy, 133) }
                                        ,{ {135}, {141}, Pose(4329+dx, 1581+dy, 134) }
                                        ,{ {6}, {142}, Pose(4529+dx, 1581+dy, 135) }
                                    // 交换x,y坐标后，以下坐标的坐标系为x正方向为朝上，y正方向为朝左
                                    // { {2}, {3}, Pose(800+dx, -3654+dy, 1) }
                                    // ,{ {1, 3}, {2, 4}, Pose(874+dx, -3654+dy, 2) }
                                    // ,{ {2, 42}, {1, 5}, Pose(1074+dx, -3836+dy, 3) }
                                    // ,{ {5}, {6}, Pose(1074+dx, -4186+dy, 4) }
                                    // ,{ {6}, {7}, Pose(1274+dx, -4386+dy, 5) }
                                    // ,{ {7}, {8}, Pose(1581+dx, -4697+dy, 6) }
                                    // ,{ {43}, {9}, Pose(2404+dx, -5485+dy, 7) }
                                    // ,{ {9}, {10}, Pose(2651+dx, -5485+dy, 8) }
                                    // ,{ {44}, {11}, Pose(3384+dx, -4752+dy, 9) }
                                    // ,{ {47}, {12}, Pose(3384+dx, -3983+dy, 10) }
                                    // ,{ {48}, {13}, Pose(3588+dx, -3654+dy, 11) }
                                    // ,{ {50, 41}, {14, 44}, Pose(3685+dx, -3186+dy, 12) }
                                    // ,{ {56}, {15}, Pose(3685+dx, -1942+dy, 13) }
                                    // ,{ {15}, {16}, Pose(3685+dx, -840+dy, 14) }
                                    // ,{ {61}, {17}, Pose(2943+dx, -107+dy, 15) }
                                    // ,{ {17}, {18}, Pose(840+dx, -107+dy, 16) }
                                    // ,{ {71}, {19}, Pose(107+dx, -840+dy, 17) }
                                    // ,{ {19}, {20}, Pose(107+dx, -1942+dy, 18) }
                                    // ,{ {76}, {21}, Pose(840+dx, -2679+dy, 19) }
                                    // ,{ {21}, {22}, Pose(1074+dx, -2679+dy, 20) }
                                    // ,{ {22}, {23}, Pose(1581+dx, -3186+dy, 21) }
                                    // ,{ {23, 130}, {24, 46}, Pose(1581+dx, -3329+dy, 22) }
                                    // ,{ {3}, {25}, Pose(1274+dx, -3636+dy, 23) }
                                    // ,{ {25, 34}, {26, 47}, Pose(1897+dx, -3186+dy, 24) }
                                    // ,{ {77}, {27}, Pose(2404+dx, -2679+dy, 25) }
                                    // ,{ {27, 13}, {28, 48}, Pose(2877+dx, -2679+dy, 26) }
                                    // ,{ {79}, {29}, Pose(3384+dx, -3186+dy, 27) }
                                    // ,{ {81}, {30}, Pose(3308+dx, -3591+dy, 28) }
                                    // ,{ {82}, {31}, Pose(3158+dx, -3901+dy, 29) }
                                    // ,{ {31}, {32}, Pose(3158+dx, -4752+dy, 30) }
                                    // ,{ {86}, {33}, Pose(2651+dx, -5259+dy, 31) }
                                    // ,{ {33}, {34}, Pose(2404+dx, -5259+dy, 32) }
                                    // ,{ {87}, {35}, Pose(1897+dx, -4697+dy, 33) }
                                    // ,{ {35}, {36}, Pose(840+dx, -2453+dy, 34) }
                                    // ,{ {94}, {37}, Pose(333+dx, -1942+dy, 35) }
                                    // ,{ {37}, {38}, Pose(333+dx, -840+dy, 36) }
                                    // ,{ {99}, {39}, Pose(840+dx, -333+dy, 37) }
                                    // ,{ {39}, {40}, Pose(2943+dx, -333+dy, 38) }
                                    // ,{ {109}, {41}, Pose(3459+dx, -840+dy, 39) }
                                    // ,{ {41, 124}, {42, 45}, Pose(3459+dx, -1942+dy, 40) }
                                    // ,{ {114}, {43}, Pose(2877+dx, -2453+dy, 41) }
                                    // ,{ {4}, {49}, Pose(1074+dx, -4036+dy, 42) }
                                    // ,{ {8}, {50}, Pose(2604+dx, -5485+dy, 43) }
                                    // ,{ {45}, {51}, Pose(3384+dx, -4552+dy, 44) }
                                    // ,{ {46}, {52}, Pose(3384+dx, -4352+dy, 45) }
                                    // ,{ {10}, {53}, Pose(3384+dx, -4152+dy, 46) }
                                    // ,{ {11}, {54}, Pose(3489+dx, -3813+dy, 47) }
                                    // ,{ {49}, {55}, Pose(3628+dx, -3458+dy, 48) }
                                    // ,{ {12}, {56}, Pose(3668+dx, -3262+dy, 49) }
                                    // ,{ {51}, {57}, Pose(3685+dx, -2986+dy, 50) }
                                    // ,{ {52}, {58}, Pose(3685+dx, -2786+dy, 51) }
                                    // ,{ {53}, {59}, Pose(3685+dx, -2586+dy, 52) }
                                    // ,{ {54}, {60}, Pose(3685+dx, -2386+dy, 53) }
                                    // ,{ {55}, {61}, Pose(3685+dx, -2186+dy, 54) }
                                    // ,{ {13}, {62}, Pose(3685+dx, -1986+dy, 55) }
                                    // ,{ {57}, {63}, Pose(3685+dx, -1742+dy, 56) }
                                    // ,{ {58}, {64}, Pose(3685+dx, -1542+dy, 57) }
                                    // ,{ {59}, {65}, Pose(3685+dx, -1342+dy, 58) }
                                    // ,{ {60}, {66}, Pose(3685+dx, -1142+dy, 59) }
                                    // ,{ {14}, {67}, Pose(3685+dx, -942+dy, 60) }
                                    // ,{ {62}, {68}, Pose(2743+dx, -107+dy, 61) }
                                    // ,{ {63}, {69}, Pose(2543+dx, -107+dy, 62) }
                                    // ,{ {64}, {70}, Pose(2343+dx, -107+dy, 63) }
                                    // ,{ {65}, {71}, Pose(2143+dx, -107+dy, 64) }
                                    // ,{ {66}, {72}, Pose(1943+dx, -107+dy, 65) }
                                    // ,{ {67}, {73}, Pose(1743+dx, -107+dy, 66) }
                                    // ,{ {68}, {74}, Pose(1543+dx, -107+dy, 67) }
                                    // ,{ {69}, {75}, Pose(1343+dx, -107+dy, 68) }
                                    // ,{ {70}, {76}, Pose(1143+dx, -107+dy, 69) }
                                    // ,{ {16}, {77}, Pose(943+dx, -107+dy, 70) }
                                    // ,{ {72}, {78}, Pose(107+dx, -1040+dy, 71) }
                                    // ,{ {73}, {79}, Pose(107+dx, -1240+dy, 72) }
                                    // ,{ {74}, {80}, Pose(107+dx, -1440+dy, 73) }
                                    // ,{ {75}, {81}, Pose(107+dx, -1640+dy, 74) }
                                    // ,{ {18}, {82}, Pose(107+dx, -1840+dy, 75) }
                                    // ,{ {20}, {83}, Pose(1040+dx, -2679+dy, 76) }
                                    // ,{ {78}, {84}, Pose(2604+dx, -2679+dy, 77) }
                                    // ,{ {26}, {85}, Pose(2804+dx, -2679+dy, 78) }
                                    // ,{ {80}, {86}, Pose(3347+dx, -3382+dy, 79) }
                                    // ,{ {28}, {87}, Pose(3310+dx, -3578+dy, 80) }
                                    // ,{ {29}, {88}, Pose(3220+dx, -3771+dy, 81) }
                                    // ,{ {83}, {89}, Pose(3158+dx, -4101+dy, 82) }
                                    // ,{ {84}, {90}, Pose(3158+dx, -4301+dy, 83) }
                                    // ,{ {85}, {91}, Pose(3158+dx, -4501+dy, 84) }
                                    // ,{ {30}, {92}, Pose(3158+dx, -4701+dy, 85) }
                                    // ,{ {32}, {93}, Pose(2451+dx, -5259+dy, 86) }
                                    // ,{ {88}, {94}, Pose(1897+dx, -4497+dy, 87) }
                                    // ,{ {89}, {95}, Pose(1897+dx, -4297+dy, 88) }
                                    // ,{ {90}, {96}, Pose(1897+dx, -4097+dy, 89) }
                                    // ,{ {91}, {97}, Pose(1897+dx, -3897+dy, 90) }
                                    // ,{ {92}, {98}, Pose(1897+dx, -3697+dy, 91) }
                                    // ,{ {93}, {99}, Pose(1897+dx, -3497+dy, 92) }
                                    // ,{ {24}, {100}, Pose(1897+dx, -3297+dy, 93) }
                                    // ,{ {95}, {101}, Pose(333+dx, -1742+dy, 94) }
                                    // ,{ {96}, {102}, Pose(333+dx, -1542+dy, 95) }
                                    // ,{ {97}, {103}, Pose(333+dx, -1342+dy, 96) }
                                    // ,{ {98}, {104}, Pose(333+dx, -1142+dy, 97) }
                                    // ,{ {36}, {105}, Pose(333+dx, -942+dy, 98) }
                                    // ,{ {100}, {106}, Pose(1040+dx, -333+dy, 99) }
                                    // ,{ {101}, {107}, Pose(1240+dx, -333+dy, 100) }
                                    // ,{ {102}, {108}, Pose(1440+dx, -333+dy, 101) }
                                    // ,{ {103}, {109}, Pose(1640+dx, -333+dy, 102) }
                                    // ,{ {104}, {110}, Pose(1840+dx, -333+dy, 103) }
                                    // ,{ {105}, {111}, Pose(2040+dx, -333+dy, 104) }
                                    // ,{ {106}, {112}, Pose(2240+dx, -333+dy, 105) }
                                    // ,{ {107}, {113}, Pose(2440+dx, -333+dy, 106) }
                                    // ,{ {108}, {114}, Pose(2640+dx, -333+dy, 107) }
                                    // ,{ {38}, {115}, Pose(2840+dx, -333+dy, 108) }
                                    // ,{ {110}, {116}, Pose(3459+dx, -1040+dy, 109) }
                                    // ,{ {111}, {117}, Pose(3459+dx, -1240+dy, 110) }
                                    // ,{ {112}, {118}, Pose(3459+dx, -1440+dy, 111) }
                                    // ,{ {113}, {119}, Pose(3459+dx, -1640+dy, 112) }
                                    // ,{ {40}, {120}, Pose(3459+dx, -1840+dy, 113) }
                                    // ,{ {115}, {121}, Pose(2677+dx, -2453+dy, 114) }
                                    // ,{ {116}, {122}, Pose(2477+dx, -2453+dy, 115) }
                                    // ,{ {117}, {123}, Pose(2277+dx, -2453+dy, 116) }
                                    // ,{ {118}, {124}, Pose(2077+dx, -2453+dy, 117) }
                                    // ,{ {119}, {125}, Pose(1877+dx, -2453+dy, 118) }
                                    // ,{ {120}, {126}, Pose(1677+dx, -2453+dy, 119) }
                                    // ,{ {121}, {127}, Pose(1477+dx, -2453+dy, 120) }
                                    // ,{ {122}, {128}, Pose(1277+dx, -2453+dy, 121) }
                                    // ,{ {123}, {129}, Pose(1077+dx, -2453+dy, 122) }
                                    // ,{ {34}, {130}, Pose(877+dx, -2453+dy, 123) }
                                    // ,{ {125}, {131}, Pose(3446+dx, -2141+dy, 124) }
                                    // ,{ {126}, {132}, Pose(3433+dx, -2340+dy, 125) }
                                    // ,{ {127}, {133}, Pose(3420+dx, -2539+dy, 126) }
                                    // ,{ {128}, {134}, Pose(3407+dx, -2738+dy, 127) }
                                    // ,{ {129}, {135}, Pose(3394+dx, -2937+dy, 128) }
                                    // ,{ {27}, {136}, Pose(3381+dx, -3136+dy, 129) }
                                    // ,{ {131}, {137}, Pose(1581+dx, -3529+dy, 130) }
                                    // ,{ {132}, {138}, Pose(1581+dx, -3729+dy, 131) }
                                    // ,{ {133}, {139}, Pose(1581+dx, -3929+dy, 132) }
                                    // ,{ {134}, {140}, Pose(1581+dx, -4129+dy, 133) }
                                    // ,{ {135}, {141}, Pose(1581+dx, -4329+dy, 134) }
                                    // ,{ {6}, {142}, Pose(1581+dx, -4529+dy, 135) }
                                    };

//定义拓扑地图的边 
//边属性1：1代表直行，2代表左转弯，3代表右转弯
//边属性2：直行时代表路段长度，转弯时代表路段半径，单位：mm
//边属性3：直行时取0，转弯时取转弯的角度，单位：度
//边属性4：计算最短路径时使用的代价值
//边属性5：入库时，是否属于倒车边，车库外的边默认false
    //                              { id, {起点，终点}，边属性1，边属性2，边属性3，边属性4 } 
    vector<Edge> mapEdgeInfoS {      
                                    // { 1, {3,2}, 2, 200, 90, 314  },
                                    // { 2, {2,1}, 1, 74, 0, 74  },
                                    // { 3, {1,2}, 1, 74, 0, 74  },
                                    // { 4, {2,3}, 3, 200, 90, 314  },
                                    // { 5, {3,4}, 1, 350, 0, 350  },
                                    // { 6, {4,5}, 2, 200, 90, 314  },
                                    // { 7, {5,6}, 3, 307, 90, 482  },
                                    // { 8, {6,7}, 2, 820, 90, 1288  },
                                    // { 9, {7,8}, 1, 247, 0, 247  },
                                    // { 10, {8,9}, 2, 733, 90, 1151  },
                                    // { 11, {9,10}, 1, 769, 0, 769  },
                                    // { 12, {10,11}, 1, 387, 0, 387 },
                                    // { 13, {11,12}, 1, 477, 0, 477 },
                                    // { 14, {12,13}, 1, 1244, 0, 1244  },
                                    // { 15, {13,14}, 1, 1102, 0, 1102  },
                                    // { 16, {14,15}, 2, 733, 90, 1151  },
                                    // { 17, {15,16}, 1, 2103, 0, 2103  },
                                    // { 18, {16,17}, 2, 733, 90, 1151  },
                                    // { 19, {17,18}, 1, 1102, 0, 1102  }, 
                                    // { 20, {18,19}, 2, 733, 90, 1151  },
                                    // { 21, {19,20}, 1, 234, 0, 234 },
                                    // { 22, {20,21}, 3, 507, 90, 796  },
                                    // { 23, {21,22}, 1, 143, 0, 143 },
                                    // { 24, {22,23}, 3, 307, 90, 482  },
                                    // { 25, {23,3}, 2, 200, 90, 314  },
                                    // { 26, {24,25}, 3, 507, 90,796  },
                                    // { 27, {25,26}, 1, 473, 0, 473 },
                                    // { 28, {26,27}, 3, 507, 90, 796  },
                                    // { 29, {27,28}, 1, 412, 0, 412  },
                                    // { 30, {28,29}, 1, 344, 0, 344  },
                                    // { 31, {29,30}, 1, 851, 0, 851 },
                                    // { 32, {30,31}, 3, 507, 90, 796  },
                                    // { 33, {31,32}, 1, 247, 0, 247  },
                                    // { 34, {32,33}, 3, 507, 90, 796 }, 
                                    // { 35, {33,24}, 1, 1511, 0, 1511 },
                                    // { 36, {34,35}, 3, 507, 90, 796  },
                                    // { 37, {35,36}, 1, 1102, 0, 1102 },
                                    // { 38, {36,37}, 3, 507, 90, 796 },
                                    // { 39, {37,38}, 1, 2103, 0, 2103 },
                                    // { 40, {38,39}, 3, 507, 90, 796 },
                                    // { 41, {39,40}, 1, 1102, 0, 1102 },
                                    // { 42, {40,41}, 3, 507, 90, 796 },
                                    // { 43, {41,34}, 1, 2037, 0, 2037 },
                                    // { 44, {12,41}, 2, 733, 90, 1151 },
                                    // { 45, {40,27}, 1, 1246, 0, 1246 },
                                    // { 46, {22,6}, 1, 1368, 0, 1368 },
                                    // { 47, {24,34}, 2, 817, 90, 1283 },
                                    // { 48, {26,13}, 2, 733, 90, 1151 }
                                    // 加密后改变的初始的边
                                    { 1, {3, 2}, 2, 200, 90, 314 }
                                    ,{ 2, {2, 1}, 1, 74, 0, 74 }
                                    ,{ 3, {1, 2}, 1, 74, 0, 74 }
                                    ,{ 4, {2, 3}, 3, 200, 90, 314 }
                                    ,{ 5, {3, 42}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 6, {4, 5}, 2, 200, 90, 314 }
                                    ,{ 7, {5, 6}, 3, 307, 90, 482 }
                                    ,{ 8, {6, 7}, 2, 820, 90, 1288 }
                                    ,{ 9, {7, 43}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 10, {8, 9}, 2, 733, 90, 1151 }
                                    ,{ 11, {9, 44}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 12, {10, 47}, 1, 199, 0, 199 }
                                    // changed!
                                    ,{ 13, {11, 48}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 14, {12, 50}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 15, {13, 56}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 16, {14, 15}, 2, 700, 90, 1151 }
                                    ,{ 17, {15, 61}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 18, {16, 17}, 2, 733, 90, 1151 }
                                    ,{ 19, {17, 71}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 20, {18, 19}, 2, 733, 90, 1151 }
                                    ,{ 21, {19, 76}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 22, {20, 21}, 3, 507, 90, 796 }
                                    ,{ 23, {21, 22}, 1, 143, 0, 143 }
                                    ,{ 24, {22, 23}, 3, 307, 90, 482 }
                                    ,{ 25, {23, 3}, 2, 200, 90, 314 }
                                    ,{ 26, {24, 25}, 3, 507, 90, 796 }
                                    ,{ 27, {25, 77}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 28, {26, 27}, 3, 507, 90, 796 }
                                    ,{ 29, {27, 79}, 1, 199, 0, 199 }
                                    // changed!
                                    ,{ 30, {28, 81}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 31, {29, 82}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 32, {30, 31}, 3, 507, 90, 796 }
                                    ,{ 33, {31, 86}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 34, {32, 33}, 3, 507, 90, 796 }
                                    ,{ 35, {33, 87}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 36, {34, 35}, 3, 507, 90, 796 }
                                    ,{ 37, {35, 94}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 38, {36, 37}, 3, 507, 90, 796 }
                                    ,{ 39, {37, 99}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 40, {38, 39}, 3, 507, 90, 796 }
                                    ,{ 41, {39, 109}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 42, {40, 41}, 3, 507, 90, 796 }
                                    ,{ 43, {41, 114}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 44, {12, 41}, 2, 733, 90, 1151 }
                                    ,{ 45, {40, 124}, 1, 199, 0, 199 }
                                    // changed!
                                    ,{ 46, {22, 130}, 1, 200, 0, 200 }
                                    // changed!
                                    ,{ 47, {24, 34}, 2, 817, 90, 1283 }
                                    ,{ 48, {26, 13}, 2, 733, 90, 1151 }
                                    // 加密获得的edge
                                        ,{ 49, {42, 4}, 1, 150, 0, 150 }
                                        ,{ 50, {43, 8}, 1, 47, 0, 47 }
                                        ,{ 51, {44, 45}, 1, 200, 0, 200 }
                                        ,{ 52, {45, 46}, 1, 200, 0, 200 }
                                        ,{ 53, {46, 10}, 1, 169, 0, 169 }
                                        ,{ 54, {47, 11}, 1, 187, 0, 187 }
                                        ,{ 55, {48, 49}, 1, 200, 0, 200 }
                                        ,{ 56, {49, 12}, 1, 77, 0, 77 }
                                        ,{ 57, {50, 51}, 1, 200, 0, 200 }
                                        ,{ 58, {51, 52}, 1, 200, 0, 200 }
                                        ,{ 59, {52, 53}, 1, 200, 0, 200 }
                                        ,{ 60, {53, 54}, 1, 200, 0, 200 }
                                        ,{ 61, {54, 55}, 1, 200, 0, 200 }
                                        ,{ 62, {55, 13}, 1, 44, 0, 44 }
                                        ,{ 63, {56, 57}, 1, 200, 0, 200 }
                                        ,{ 64, {57, 58}, 1, 200, 0, 200 }
                                        ,{ 65, {58, 59}, 1, 200, 0, 200 }
                                        ,{ 66, {59, 60}, 1, 200, 0, 200 }
                                        ,{ 67, {60, 14}, 1, 102, 0, 102 }
                                        ,{ 68, {61, 62}, 1, 200, 0, 200 }
                                        ,{ 69, {62, 63}, 1, 200, 0, 200 }
                                        ,{ 70, {63, 64}, 1, 200, 0, 200 }
                                        ,{ 71, {64, 65}, 1, 200, 0, 200 }
                                        ,{ 72, {65, 66}, 1, 200, 0, 200 }
                                        ,{ 73, {66, 67}, 1, 200, 0, 200 }
                                        ,{ 74, {67, 68}, 1, 200, 0, 200 }
                                        ,{ 75, {68, 69}, 1, 200, 0, 200 }
                                        ,{ 76, {69, 70}, 1, 200, 0, 200 }
                                        ,{ 77, {70, 16}, 1, 103, 0, 103 }
                                        ,{ 78, {71, 72}, 1, 200, 0, 200 }
                                        ,{ 79, {72, 73}, 1, 200, 0, 200 }
                                        ,{ 80, {73, 74}, 1, 200, 0, 200 }
                                        ,{ 81, {74, 75}, 1, 200, 0, 200 }
                                        ,{ 82, {75, 18}, 1, 102, 0, 102 }
                                        ,{ 83, {76, 20}, 1, 34, 0, 34 }
                                        ,{ 84, {77, 78}, 1, 200, 0, 200 }
                                        ,{ 85, {78, 26}, 1, 73, 0, 73 }
                                        ,{ 86, {79, 80}, 1, 199, 0, 199 }
                                        ,{ 87, {80, 28}, 1, 13, 0, 13 }
                                        ,{ 88, {81, 29}, 1, 144, 0, 144 }
                                        ,{ 89, {82, 83}, 1, 200, 0, 200 }
                                        ,{ 90, {83, 84}, 1, 200, 0, 200 }
                                        ,{ 91, {84, 85}, 1, 200, 0, 200 }
                                        ,{ 92, {85, 30}, 1, 51, 0, 51 }
                                        ,{ 93, {86, 32}, 1, 47, 0, 47 }
                                        ,{ 94, {87, 88}, 1, 200, 0, 200 }
                                        ,{ 95, {88, 89}, 1, 200, 0, 200 }
                                        ,{ 96, {89, 90}, 1, 200, 0, 200 }
                                        ,{ 97, {90, 91}, 1, 200, 0, 200 }
                                        ,{ 98, {91, 92}, 1, 200, 0, 200 }
                                        ,{ 99, {92, 93}, 1, 200, 0, 200 }
                                        ,{ 100, {93, 24}, 1, 111, 0, 111 }
                                        ,{ 101, {94, 95}, 1, 200, 0, 200 }
                                        ,{ 102, {95, 96}, 1, 200, 0, 200 }
                                        ,{ 103, {96, 97}, 1, 200, 0, 200 }
                                        ,{ 104, {97, 98}, 1, 200, 0, 200 }
                                        ,{ 105, {98, 36}, 1, 102, 0, 102 }
                                        ,{ 106, {99, 100}, 1, 200, 0, 200 }
                                        ,{ 107, {100, 101}, 1, 200, 0, 200 }
                                        ,{ 108, {101, 102}, 1, 200, 0, 200 }
                                        ,{ 109, {102, 103}, 1, 200, 0, 200 }
                                        ,{ 110, {103, 104}, 1, 200, 0, 200 }
                                        ,{ 111, {104, 105}, 1, 200, 0, 200 }
                                        ,{ 112, {105, 106}, 1, 200, 0, 200 }
                                        ,{ 113, {106, 107}, 1, 200, 0, 200 }
                                        ,{ 114, {107, 108}, 1, 200, 0, 200 }
                                        ,{ 115, {108, 38}, 1, 103, 0, 103 }
                                        ,{ 116, {109, 110}, 1, 200, 0, 200 }
                                        ,{ 117, {110, 111}, 1, 200, 0, 200 }
                                        ,{ 118, {111, 112}, 1, 200, 0, 200 }
                                        ,{ 119, {112, 113}, 1, 200, 0, 200 }
                                        ,{ 120, {113, 40}, 1, 102, 0, 102 }
                                        ,{ 121, {114, 115}, 1, 200, 0, 200 }
                                        ,{ 122, {115, 116}, 1, 200, 0, 200 }
                                        ,{ 123, {116, 117}, 1, 200, 0, 200 }
                                        ,{ 124, {117, 118}, 1, 200, 0, 200 }
                                        ,{ 125, {118, 119}, 1, 200, 0, 200 }
                                        ,{ 126, {119, 120}, 1, 200, 0, 200 }
                                        ,{ 127, {120, 121}, 1, 200, 0, 200 }
                                        ,{ 128, {121, 122}, 1, 200, 0, 200 }
                                        ,{ 129, {122, 123}, 1, 200, 0, 200 }
                                        ,{ 130, {123, 34}, 1, 37, 0, 37 }
                                        ,{ 131, {124, 125}, 1, 199, 0, 199 }
                                        ,{ 132, {125, 126}, 1, 199, 0, 199 }
                                        ,{ 133, {126, 127}, 1, 199, 0, 199 }
                                        ,{ 134, {127, 128}, 1, 199, 0, 199 }
                                        ,{ 135, {128, 129}, 1, 199, 0, 199 }
                                        ,{ 136, {129, 27}, 1, 50, 0, 50 }
                                        ,{ 137, {130, 131}, 1, 200, 0, 200 }
                                        ,{ 138, {131, 132}, 1, 200, 0, 200 }
                                        ,{ 139, {132, 133}, 1, 200, 0, 200 }
                                        ,{ 140, {133, 134}, 1, 200, 0, 200 }
                                        ,{ 141, {134, 135}, 1, 200, 0, 200 }
                                        ,{ 142, {135, 6}, 1, 168, 0, 168 }
                                         };



//定义停车场拓扑地图的顶点
    //                              { {可到达的顶点}, {对应的边},  Pose{x,y,id}}
    vector<Vertex> parkVertexInfoS{ { {0,0},  {0,0},  Pose(0,0,201) } };

//定义拓扑地图的边    
    //                          { id, {起点，终点}，边属性1，边属性2，边属性3，边属性4，边属性5 } 
    vector<Edge> parkEdgeInfoS { { 201,  {201,202}, 1, 55, 0, 55, true} };
                                
                                
//******************************
//          功能函数
//******************************  
    const vector<string>* getVehicleInfoS()             //获取车辆基本信息   
    {
        return &vehicleInfoS;
    }       

    const vector<vector<double>>* getTLInfoS()          //获取信号灯相关信息
    {
        return &TLInfoS;
    }    

    const vector<Vertex>* getMapVertexInfoS()           //获取道路节点数据
    {
        return &mapVertexInfoS;
    }
    const vector<Edge>* getMapEdgeInfoS()               //获取道路边数据
    {
        return &mapEdgeInfoS;
    }

    const vector<Vertex>* getParkVertexInfoS()          //获取停车场节点数据
    {
        return &parkVertexInfoS;
    }
    const vector<Edge>* getParkEdgeInfoS()              //获取停车场边数据
    {
        return &parkEdgeInfoS;
    }
    
    
